# pragma once

# include <iostream>
# include <cstdint>
# include <vector>
# include <math.h>
# include "serialib.h"


namespace WHEELTEC_N100
{
    //创建IMU数据类型的类
    struct IMUPacket{
        float gyroscope_x;          //unit: rad/s
        float gyroscope_y;          //unit: rad/s
        float gyroscope_z;          //unit: rad/s
        float accelerometer_x;      //m/s^2
        float accelerometer_y;      //m/s^2
        float accelerometer_z;      //m/s^2
        float magnetometer_x;       //mG
        float magnetometer_y;       //mG
        float magnetometer_z;       //mG
        float imu_temperature;      //C
        float Pressure;             //Pa
        float pressure_temperature; //C
        uint32_t Timestamp;         //us
    };
    
    //创建AHRS数据类型的类
    class AHRSPacket{
     public:
      float RollSpeed;   //unit: rad/s
      float PitchSpeed;  //unit: rad/s
      float HeadingSpeed;//unit: rad/s
      float Roll;        //unit: rad
      float Pitch;       //unit: rad
      float Heading;     //unit: rad
      float Qw;//w          //Quaternion
      float Qx;//x
      float Qy;//y
      float Qz;//z
      uint32_t Timestamp; //unit: us
    };
    
    class driver
    {
    private:
        //N100惯导数据格式宏定义
        uint8_t frame_head = 0xFC; //数据帧头
        uint8_t frame_end = 0xFD; //数据帧尾
    
        uint8_t imu_len = 0x38;  //IMU数据长度
        uint8_t imu_type = 0x40; //IMU数据类别
        int imu_data_len = 64; //当数据类型为IMU时，该数组的总长度
    
        uint8_t ahrs_len = 0x30; //AHRS数据长度
        uint8_t ahrs_type = 0x41; //AHRS数据类别
        int ahrs_data_len = 56; //当数据类型为AHRS时，该数组的总长度
    
        IMUPacket imu_packet;
        AHRSPacket ahrs_packet;
    
        std::vector<uint8_t> mem_data;
        serialib serial;
        
        std::string device;
        int bauds;
    
        //16进制转浮点数实现函数
        float HEX_to_Float(uint8_t data1,uint8_t data2,uint8_t data3,uint8_t data4)
        {
            //数据由高位到地位排序 ： data1 data2 data3 data4
            //其中接收数据时，高位存放在后面，低位存放在前面
            
            short sign; //符号计算
            unsigned long temp; //32位16进制数
            short zhishu; //存放指数
            float weishu;    //存放尾数
            float ref;       //存放计算结果
            uint16_t H_16,L_16; //存放高16位、低16位
    
            H_16 = data1<<8 | data2;//数据第一次融合
            L_16 = data3<<8 | data4;
    
            //将融合成16位的数据组合成32位数据
            temp = (unsigned long)H_16<<16|(unsigned long)L_16;
    
            //浮点数转化开始
            //先确定符号
            sign = (temp & 0x80000000) ? -1 : 1;  //最高位是1就是负数，0就是正数
            //计算指数
            // 127是偏移量，使得指数有正负（指数的范围是 -127 ~ +128 ）
            zhishu = ((temp >> 23) & 0xff) - 127;
            
            //获取尾数部分 将（temp & 0x7fffff）获得的值将小数点左移23位 (除以2的23次方)
            weishu = 1 + ((float)(temp & 0x7fffff) / 0x800000);
            
            //最终的公式
            ref = sign * weishu * std::pow(2, zhishu);
            return ref;
        }
    
        float HEX_to_Float_New(uint8_t *data,bool mode)
        {
            float fa=0;
            uint8_t uc[4];
    
            if(mode==false)
            {
                uc[3] = data[0];
                uc[2] = data[1];
                uc[1] = data[2];
                uc[0] = data[3];
            }
            else
            {
                uc[0] = data[0];
                uc[1] = data[1];
                uc[2] = data[2];
                uc[3] = data[3];
            }
            memcpy(&fa,uc,4);
            return fa;
        }
    
        //时间戳解包函数
        long long timestamp(uint8_t Data_1,uint8_t Data_2,uint8_t Data_3,uint8_t Data_4)
        {
            unsigned long temp; //32位16进制数
            uint16_t H_16,L_16; //存放高16位、低16位
    
            H_16 = Data_1 << 8 | Data_2;
            L_16 = Data_3 << 8 | Data_4;
    
            //将融合成16位的数据组合成32位数据
            temp = (unsigned long)H_16<<16|(unsigned long)L_16;
    
            return temp;
        }
    
        void UnpackIMUData(std::vector<uint8_t> & IMU_Data)
        {
            uint8_t temp[4];
            temp[0] = IMU_Data[7] , temp[1] = IMU_Data[8] , temp[2] = IMU_Data[9], temp[3] = IMU_Data[10];
            imu_packet.gyroscope_x = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[11] , temp[1] = IMU_Data[12] , temp[2] = IMU_Data[13], temp[3] = IMU_Data[14];
            imu_packet.gyroscope_y = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[15] , temp[1] = IMU_Data[16] , temp[2] = IMU_Data[17], temp[3] = IMU_Data[18];
            imu_packet.gyroscope_z = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[19] , temp[1] = IMU_Data[20] , temp[2] = IMU_Data[21], temp[3] = IMU_Data[22];
            imu_packet.accelerometer_x = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[23] , temp[1] = IMU_Data[24] , temp[2] = IMU_Data[25], temp[3] = IMU_Data[26];
            imu_packet.accelerometer_y = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[27] , temp[1] = IMU_Data[28] , temp[2] = IMU_Data[29], temp[3] = IMU_Data[30];
            imu_packet.accelerometer_z = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[31] , temp[1] = IMU_Data[32] , temp[2] = IMU_Data[33], temp[3] = IMU_Data[34];
            imu_packet.magnetometer_x = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[35] , temp[1] = IMU_Data[36] , temp[2] = IMU_Data[37], temp[3] = IMU_Data[38];
            imu_packet.magnetometer_y = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[39] , temp[1] = IMU_Data[40] , temp[2] = IMU_Data[41], temp[3] = IMU_Data[42];
            imu_packet.magnetometer_z = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[43] , temp[1] = IMU_Data[44] , temp[2] = IMU_Data[45], temp[3] = IMU_Data[46];
            imu_packet.imu_temperature = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[47] , temp[1] = IMU_Data[48] , temp[2] = IMU_Data[49], temp[3] = IMU_Data[50];
            imu_packet.Pressure = HEX_to_Float_New(temp,true);
    
            temp[0] = IMU_Data[51] , temp[1] = IMU_Data[52] , temp[2] = IMU_Data[53], temp[3] = IMU_Data[54];
            imu_packet.pressure_temperature = HEX_to_Float_New(temp,true);
    
            imu_packet.Timestamp = timestamp(IMU_Data[58],IMU_Data[57],IMU_Data[56],IMU_Data[55]);
        }
    
        void UnpackAHRSData(std::vector<uint8_t> & AHRS_Data)
        {
            uint8_t temp[4];
            temp[0] = AHRS_Data[7] , temp[1] = AHRS_Data[8] , temp[2] = AHRS_Data[9], temp[3] = AHRS_Data[10];
            ahrs_packet.RollSpeed = HEX_to_Float_New(temp,true);
    
            temp[0] = AHRS_Data[11] , temp[1] = AHRS_Data[12] , temp[2] = AHRS_Data[13], temp[3] = AHRS_Data[14];
            ahrs_packet.PitchSpeed = HEX_to_Float_New(temp,true);
    
            temp[0] = AHRS_Data[15] , temp[1] = AHRS_Data[16] , temp[2] = AHRS_Data[17], temp[3] = AHRS_Data[18];
            ahrs_packet.HeadingSpeed = HEX_to_Float_New(temp,true);
    
            temp[0] = AHRS_Data[19] , temp[1] = AHRS_Data[20] , temp[2] = AHRS_Data[21], temp[3] = AHRS_Data[22];
            ahrs_packet.Roll = HEX_to_Float_New(temp,true);
    
            temp[0] = AHRS_Data[23] , temp[1] = AHRS_Data[24] , temp[2] = AHRS_Data[25], temp[3] = AHRS_Data[26];
            ahrs_packet.Pitch = HEX_to_Float_New(temp,true);
    
            temp[0] = AHRS_Data[27] , temp[1] = AHRS_Data[28] , temp[2] = AHRS_Data[29], temp[3] = AHRS_Data[30];
            ahrs_packet.Heading = HEX_to_Float_New(temp,true);
    
            temp[0] = AHRS_Data[31] , temp[1] = AHRS_Data[32] , temp[2] = AHRS_Data[33], temp[3] = AHRS_Data[34];
            ahrs_packet.Qw = HEX_to_Float_New(temp,true);
    
            temp[0] = AHRS_Data[35] , temp[1] = AHRS_Data[36] , temp[2] = AHRS_Data[37], temp[3] = AHRS_Data[38];
            ahrs_packet.Qx = HEX_to_Float_New(temp,true);
    
            temp[0] = AHRS_Data[39] , temp[1] = AHRS_Data[40] , temp[2] = AHRS_Data[41], temp[3] = AHRS_Data[42];
            ahrs_packet.Qy = HEX_to_Float_New(temp,true);
    
            temp[0] = AHRS_Data[43] , temp[1] = AHRS_Data[44] , temp[2] = AHRS_Data[45], temp[3] = AHRS_Data[46];
            ahrs_packet.Qz = HEX_to_Float_New(temp,true);
            ahrs_packet.Timestamp = timestamp(AHRS_Data[50],AHRS_Data[49],AHRS_Data[48],AHRS_Data[47]); //unit: us
        }
    
    public:
        /// @brief 实例化一个imu驱动对象
        /// @param arg1 device 设备路径，如："/dev/ttyUSB0"
        /// @param arg2 bauds 波特率，如：921600
        driver(const char * arg1, int arg2)
            :device(arg1), bauds(arg2)
        {
            char status = serial.openDevice(device.data(),bauds);
            if (status != 1)
            {
                throw std::runtime_error("imu driver error: open device: " + device);
            }
    
            std::cout << "imu driver online at " << device << std::endl;
        };
        ~driver()
        {
            serial.closeDevice();
        };
    
        /// @brief 获取端口处的imu数据
        /// @return 
        //      -2：端口没有打开
        //      -1: 没有检查到有效头；
        //      0：检查到有效头，但是长度不够；
        //      1：检查到IMU数据的有效数据帧
        //      2：检查到AHRS数据的有效数据帧
        int reciveData()
        {
            // std::cout << mem_data.size() << std::endl; // 有没有出现数据缓存问题就看这个
    
            std::vector<uint8_t> catData;
            if (mem_data.size() > 0)
            {
                catData.insert(catData.end(),mem_data.begin(),mem_data.end());
                mem_data.clear();
            }
            
            auto length = serial.available();
            if (length >0)
            {
                std::vector<uint8_t> buffer(length);
                if (serial.isDeviceOpen())
                {
                    serial.readBytes(buffer.data(),length);
                }
                else
                {
                    std::cout << "imu driver error:  device not open ()" << std::endl;
        
                    return -2;
                }
    
                catData.insert(catData.end(),buffer.begin(),buffer.end());
            }
    
            int n = 0;
            int n_bytes = catData.size();
            if (n_bytes == 0)
            {
                return -1;
            }
            while (n < n_bytes)
            {
                if (catData[n] == frame_head)
                {
                    if (n_bytes - n >= imu_data_len)
                    {
                        if (catData[n + 1] == imu_type && catData[n + 2] == imu_len && catData[n + imu_data_len - 1] == frame_end)
                        {
                            UnpackIMUData(catData);
    
                            mem_data.insert(mem_data.end(),catData.begin() + n + imu_data_len,catData.end());
    
                            return 1;
                        }
                    }
    
                    if (n_bytes - n >= ahrs_data_len)
                    {
                        if (catData[n + 1] == ahrs_type && catData[n + 2] == ahrs_len && catData[n + ahrs_data_len - 1] == frame_end)
                        {
                            UnpackAHRSData(catData);
    
                            mem_data.insert(mem_data.end(),catData.begin() + n + ahrs_data_len,catData.end());
    
                            return 2;
                        }
                    }
    
                    if (n_bytes - n < std::min(imu_data_len,ahrs_data_len))
                    {
                        mem_data.insert(mem_data.end(),catData.begin() + n,catData.end());
    
                        return 0;
                    }
                }
    
                n = n + 1;
            }
    
            return -1;
        }
    
        const IMUPacket *get_IMUdata()
        {
            return &imu_packet;
        }
    
        const AHRSPacket *get_AHRSdata()
        {
            return &ahrs_packet;
        }

        driver(const driver &) = delete;
        driver operator = (const driver &) = delete;
    };
};
